#pragma once

#ifndef _FRAME_H_
#define _FRAME_H_

#include "common_include.h"
#include"utility.h"

namespace vld_imu_slam{

struct Frame{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<Frame> Ptr;

    unsigned long keyframe_id_ =0;
    unsigned long frame_id_ =0;
    double time_stamp_;
    bool is_keyframe_ = false;

    SE3 w_pose_;

    pcl::PointCloud<PointType>::Ptr pc_surface_,pc_corner_,pc_full_,pc_surface_sample_,pc_corner_sample_;

    mutex w_pose_mutex_;

    Frame(){}
    Frame(pcl::PointCloud<PointType>::Ptr pc,pcl::PointCloud<PointType>::Ptr pc_plane,pcl::PointCloud<PointType>::Ptr pc_corner){
        pc_corner_ =pc_corner;
        pc_surface_ = pc_plane; 
        pc_full_ = pc;
        pc_surface_sample_.reset(new pcl::PointCloud<PointType>);
        pc_corner_sample_.reset(new pcl::PointCloud<PointType>);
        pcl::copyPointCloud(*pc_corner_,*pc_corner_sample_);
        pcl::copyPointCloud(*pc_surface_,*pc_surface_sample_);
        pcDownSample(pc_full_, 0.5);
        pcDownSample(pc_surface_sample_, 1.0);
        // pcDownSample(pc_corner_sample_, 0.2);
        
        // pcDownSample(pc_ground_,0.2);

        w_pose_ = SE3(Eigen::Quaterniond(1,0,0,0),Eigen::Vector3d(0,0,0));

        static long frame_factory_id = 0;
        frame_id_ = frame_factory_id++;
    }

    void setKeyFrame(){
        static long keyframe_factory_id = 0;
        is_keyframe_ = true;
        keyframe_id_ = keyframe_factory_id++;
    }

    SE3 getPose(){
        unique_lock<mutex> lck(w_pose_mutex_);
        return w_pose_;
    }

    void setPose(const SE3 &pose) {
        unique_lock<mutex> lck(w_pose_mutex_);
        w_pose_ = pose;
        // pcl::transformPointCloud(*pc_surface_,*pc_surface_,SE3toEigenM(w_pose_));
        // pcl::transformPointCloud(*pc_corner_,*pc_corner_,SE3toEigenM(w_pose_));
    }

};



}
#endif  
